package com.wltt.product.frfront.executors;

import com.wltt.product.frfront.config.exception.BusinessException;
import com.wltt.product.frfront.config.robot.WlttRobot;
import com.wltt.product.frfront.constants.RobotJoinConstant;
import com.wltt.product.frfront.constants.RobotMoveConstant;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.model.vo.JointMoveREQ;
import fairino.Robot;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.springframework.http.HttpStatus;

public class JointMoveExecutor implements Runnable {

    private static final Logger logger = LogManager.getLogger(JointMoveExecutor.class);

    /**
     * 机器人对象
     */
    private WlttRobot robot;

    /**
     * 运动参数
     */
    private JointMoveREQ jointMoveREQ;

    public JointMoveExecutor(WlttRobot robot, JointMoveREQ jointMoveREQ) {
        this.robot = robot;
        this.jointMoveREQ = jointMoveREQ;
    }

    @Override
    public void run() {
        if (!RobotHolder.setBusy()) {
            return;
        }
        if (!RobotHolder.setRobotMoving()) {
            return;
        }

        try {
            robot.Mode(0);

            int nb;
            if (RobotJoinConstant.JOIN_1.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 1;
            } else if (RobotJoinConstant.JOIN_2.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 2;
            } else if (RobotJoinConstant.JOIN_3.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 3;
            } else if (RobotJoinConstant.JOIN_4.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 4;
            } else if (RobotJoinConstant.JOIN_5.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 5;
            } else if (RobotJoinConstant.JOIN_6.equalsIgnoreCase(jointMoveREQ.getJoin())) {
                nb = 6;
            } else {
                throw new BusinessException("指定关节无效", HttpStatus.BAD_REQUEST);
            }

            int dir;
            if (RobotMoveConstant.FORWARD == jointMoveREQ.getDirection()) {
                dir = 1;
            } else if (RobotMoveConstant.REVERSE == jointMoveREQ.getDirection()) {
                dir = 0;
            } else {
                throw new BusinessException("指定方向无效", HttpStatus.BAD_REQUEST);
            }

//            int step = 0;
//            while (RobotHolder.isRobotMoving() && step < jointMoveREQ.getMaxStep()) {
//                int result = robot.StartJOG(0, nb, dir, 30, 50, 5);//关节点动
//                if (result != 0) {
//                    throw new BusinessException("运动指令执行失败", HttpStatus.BAD_REQUEST);
//                }
//                step = step + 5;
//                Thread.sleep(500);
//            }
            robot.StartJOG(0, nb, dir, 5, 50, jointMoveREQ.getMaxStep());//关节点动
        } finally {
            RobotHolder.setNoBusy();
            RobotHolder.setRobotNoMoving();
            robot.Mode(1);
        }
    }
}
